#include "dmke.h"
#include "../common/common_motor.h"

#include "../leisai-485/rs7020.h"
#include "../hcfa/hcfa.h"
#include "../../1km/driver/common/device_manager.h"

#ifndef NULL
#define NULL ((void *)0)
#endif

#if 1
struct agv_wheel {
    unsigned char motor_reverse;// 电机是否反向
    unsigned char direction;    // 轮子前进还是后退
    unsigned short speed_ratio; // 减速比
    unsigned short speed;       // cm/s
    float radius;      // 轮子半径

    struct xp_motor *motor;     // 轮子的电机对象
    
    int (*init)(struct agv_wheel *wheel);
    int (*forward)(struct agv_wheel *wheel);
    int (*backoff)(struct agv_wheel *wheel);
    int (*stop)(struct agv_wheel *wheel);
    int (*set_speed)(struct agv_wheel *wheel, unsigned short speed);     // cm/s
    int (*set_direction)(struct agv_wheel *wheel, unsigned char dir);
    int (*set_emerg)(struct agv_wheel *wheel, unsigned char emerg);
    int (*get_distance)(struct agv_wheel *wheel, int *distance);
};

static int wheel_init(struct agv_wheel *wheel)
{
    wheel->motor->ops->init(wheel->motor);
    wheel->motor->ops->speed_set(wheel->motor, 0);
    wheel->motor->ops->direction_set(wheel->motor, wheel->motor_reverse == 1 ? motor_reverser : motor_forward);

    return 0;
}
static int wheel_forward(struct agv_wheel *wheel)
{
    int ret = 0;
    if (wheel->motor->inited == 0) return -1;
    motor_direction_t motor_dir = (wheel->motor_reverse == 1) ? motor_reverser : motor_forward;
    ret = wheel->motor->ops->direction_set(wheel->motor, motor_dir);
    if (ret != 0) return -1;
    ret = wheel->motor->ops->rotation_start(wheel->motor);
    if (ret != 0) return -1;
    wheel->direction = 1;
    return 0;
}
static int wheel_backoff(struct agv_wheel *wheel)
{
    int ret = 0;
    if (wheel->motor->inited == 0) return -1;
    motor_direction_t motor_dir = (wheel->motor_reverse == 1) ? motor_forward : motor_reverser;
    ret = wheel->motor->ops->direction_set(wheel->motor, motor_dir);
    if (ret != 0) return -1;
    ret = wheel->motor->ops->rotation_start(wheel->motor);
    if (ret != 0) return -1;
    wheel->direction = 0;
    return 0;
}
static int wheel_stop(struct agv_wheel *wheel)
{
    int ret = 0;
    if (wheel->motor->inited == 0) return -1;
    return wheel->motor->ops->rotation_stop(wheel->motor);
}
static int wheel_set_speed(struct agv_wheel *wheel, unsigned short speed)
{
    float motor_speed = 0;
    float pi = 3.1416;
    if (wheel->motor->inited == 0) return -1;
    motor_speed = ((speed) / (2 * pi * wheel->radius)) * wheel->speed_ratio * 60;     // 转换为rpm
    motor_rpm_t rpm = (motor_rpm_t)motor_speed;
    int ret = wheel->motor->ops->speed_set(wheel->motor, rpm);
    if (ret == 0) wheel->speed = rpm;
    return ret;
}
static int wheel_set_direction(struct agv_wheel *wheel, unsigned char dir)
{
    if (wheel->motor->inited == 0) return -1;
    motor_direction_t motor_dir = (dir == 1) ? motor_forward : motor_reverser;
    if (wheel->motor_reverse) {
        motor_dir = (motor_dir == motor_forward) ? motor_reverser : motor_forward;
    }
    return wheel->motor->ops->direction_set(wheel->motor, motor_dir);
}
static int wheel_set_emerg(struct agv_wheel *wheel, unsigned char emerg)
{
    if (wheel->motor->inited == 0) return -1;
    int ret = 0;
    if (emerg == 1)ret = wheel->motor->ops->emerg_enable(wheel->motor);
    else ret = wheel->motor->ops->emerg_enable(wheel->motor);
    return ret;
}

struct xp_agv_wheels {
    short left_speed;   // cm/s
    short right_speed;   // cm/s
    unsigned char  reverse; // 反转前后方向

    struct agv_wheel left_wheel;
    struct agv_wheel right_wheel;
} xp_agv = {
    .left_wheel = {
        .motor_reverse = 0,
        .direction = 0,
        .speed_ratio = 21,
        .speed = 0,
        .radius = 10,
        .init = wheel_init,
        .forward = wheel_forward,
        .backoff = wheel_backoff,
        .stop = wheel_stop,
        .set_speed = wheel_set_speed,
        .set_direction = wheel_set_direction,
        .set_emerg = wheel_set_emerg,
        .motor = NULL,
    },
    .right_wheel= {
        .motor_reverse = 1,
        .direction = 0,
        .speed_ratio = 21,
        .speed = 0,
        .radius = 10,
        .init = wheel_init,
        .forward = wheel_forward,
        .backoff = wheel_backoff,
        .stop = wheel_stop,
        .set_speed = wheel_set_speed,
        .set_direction = wheel_set_direction,
        .set_emerg = wheel_set_emerg,
        .motor = NULL,
    }
};

static int xp_agv_init(struct xp_agv_wheels *agv, struct xp_motor *l_motor, struct xp_motor *r_motor)
{
    if (agv == NULL || l_motor == NULL || r_motor == NULL) return -1;
    if (l_motor->inited == 0 || r_motor->inited == 0) return -1;
    agv->left_speed = 0;
    agv->right_speed = 0;
    agv->reverse = 0;
    agv->left_wheel.motor = l_motor;
    agv->right_wheel.motor = r_motor;
    agv->left_wheel.init(&agv->left_wheel);
    agv->right_wheel.init(&agv->right_wheel);
}
static int xp_agv_forward(struct xp_agv_wheels *agv)
{
    if (agv->reverse) {
        agv->left_wheel.backoff(&agv->left_wheel);
        agv->right_wheel.backoff(&agv->right_wheel);
    } else {
        agv->left_wheel.forward(&agv->left_wheel);
        agv->right_wheel.forward(&agv->right_wheel);
    }
    return 0;
}
static int xp_agv_backoff(struct xp_agv_wheels *agv)
{
    if (agv->reverse) {
        agv->left_wheel.forward(&agv->left_wheel);
        agv->right_wheel.forward(&agv->right_wheel);
    } else {
        agv->left_wheel.backoff(&agv->left_wheel);
        agv->right_wheel.backoff(&agv->right_wheel);
    }
    return 0;
}
static int xp_agv_speed_set(struct xp_agv_wheels *agv, unsigned short speed)
{
    agv->left_speed = speed;
    agv->left_wheel.set_speed(&agv->left_wheel, speed);
    agv->right_speed = speed;
    agv->right_wheel.set_speed(&agv->right_wheel, speed);
    return 0;
}
static int xp_agv_turn_left(struct xp_agv_wheels *agv)
{
    agv->left_speed -= 5; 
    if (agv->left_speed < 0) agv->left_speed = 0;
    agv->right_speed += 5;
    if (agv->right_speed > 20) agv->right_speed = 20;
    if (agv->reverse) {
        agv->left_wheel.set_speed(&agv->left_wheel, agv->right_speed);
        agv->right_wheel.set_speed(&agv->right_wheel, agv->left_speed);
    } else {
        agv->left_wheel.set_speed(&agv->left_wheel, agv->left_speed);
        agv->right_wheel.set_speed(&agv->right_wheel, agv->right_speed);
    }

    return 0;
}
static int xp_agv_turn_right(struct xp_agv_wheels *agv)
{
    agv->left_speed += 5; 
    if (agv->left_speed > 20) agv->left_speed = 20;
    agv->right_speed -= 5;    // 1cm/s
    if (agv->right_speed < 0) agv->right_speed = 0;
    if (agv->reverse) {
        agv->left_wheel.set_speed(&agv->left_wheel, agv->right_speed);
        agv->right_wheel.set_speed(&agv->right_wheel, agv->left_speed);
    } else {
        agv->left_wheel.set_speed(&agv->left_wheel, agv->left_speed);
        agv->right_wheel.set_speed(&agv->right_wheel, agv->right_speed);
    }

    return 0;
}
static int xp_agv_stop(struct xp_agv_wheels *agv)
{
    agv->left_wheel.stop(&agv->left_wheel);
    agv->right_wheel.stop(&agv->right_wheel);
    return 0;
}
static int xp_agv_emerg_enable(struct xp_agv_wheels *agv)
{
    agv->left_wheel.set_emerg(&agv->left_wheel, 1);
    agv->right_wheel.set_emerg(&agv->right_wheel, 1);
    return 0;
}
static int xp_agv_emerg_disable(struct xp_agv_wheels *agv)
{
    agv->left_wheel.set_emerg(&agv->left_wheel, 0);
    agv->right_wheel.set_emerg(&agv->right_wheel, 0);
    return 0;
}
static int xp_agv_dir_reverse(struct xp_agv_wheels *agv)
{
    xp_agv_stop(agv);
    agv->reverse = (agv->reverse == 1) ? 0 : 1;
}

extern int xp_agv_test_debug(char *type, char *fun, char *param)
{
    if (type == NULL || fun == NULL || param == NULL) return 0;
    if (strcmp(type, "agv_test") != 0) return 0;

    struct xp_modbus_device *left_dev = get_modbus_device_by_sid(1);
    struct xp_modbus_device *right_dev = get_modbus_device_by_sid(2);

    static struct xp_motor left_motor = {0};
    static struct xp_motor right_motor = {0};

    static char agv_inited = 0;
    if (agv_inited == 0) {
        //xp_rs7020_constructor(&left_motor, left_dev, "agv_leftwheel");
        //xp_rs7020_constructor(&right_motor, right_dev, "agv_rightwheel");
        //xp_dmke_motor_constructor(&left_motor, left_dev, "agv_leftwheel");
        //xp_dmke_motor_constructor(&right_motor, right_dev, "agv_rightwheel");
        xp_hcfa_motor_constructor(&left_motor, left_dev, "agv_leftwheel");
        xp_hcfa_motor_constructor(&right_motor, right_dev, "agv_rightwheel");
        if (left_motor.inited != 1 || right_motor.inited != 1) return 0;
        xp_agv_init(&xp_agv, &left_motor, &right_motor);
        agv_inited = 1;
    }

    if (strcmp(fun, "reset") == 0) {
        agv_inited = 0;
        return 1;
    } else if (strcmp(fun, "forward") == 0) {
        xp_agv_forward(&xp_agv);
    } else if (strcmp(fun, "backoff") == 0) {
        xp_agv_backoff(&xp_agv);
    } else if (strcmp(fun, "stop") == 0) {
        xp_agv_stop(&xp_agv);
    } else if (strcmp(fun, "turn_left") == 0) {
        xp_agv_turn_left(&xp_agv);
    } else if (strcmp(fun, "turn_right") == 0) {
        xp_agv_turn_right(&xp_agv);
    } else if (strcmp(fun, "speed_set") == 0) {
        xp_agv_speed_set(&xp_agv, atoi(param));
    } else if (strcmp(fun, "emerg") == 0) {
        if (0 == atoi(param)) xp_agv_emerg_disable(&xp_agv);
        else xp_agv_emerg_enable(&xp_agv);  
    } else if (strcmp(fun, "reverse") == 0) {
        xp_agv_dir_reverse(&xp_agv);
    }
}

#endif
